#include "ros/ros.h"
#include "std_msgs/Float32MultiArray.h"
#include <vector>
#include <cmath>

#define step_length 0.05
#define dtheta (M_PI / 180)
#define ARC_RADIUS 0.5

int main(int argc, char *argv[])
{
    setlocale(LC_ALL, "");
    ros::init(argc, argv, "path_publisher");
    ros::NodeHandle nh;

    ros::Publisher pub = nh.advertise<std_msgs::Float32MultiArray>("/mpc_route", 10);
    std_msgs::Float32MultiArray route;

    std::vector<std::pair<float, float>> waypoints = {
        {0.0, 0.0},   // A
        {1.8, 0.0},  // B
        {2.4, 0.3}, // C
        {3.0, 0.3}, // D
   	{3.6, 0.0},       
        {6.3, 0.0},  // E
        {6.9, -0.6}, // G
        {6.9, -3.4}  // H
    };

    auto normalize = [](float &x, float &y) {
        float len = sqrt(x * x + y * y);
        if (len > 1e-5)
        {
            x /= len;
            y /= len;
        }
        return len;
    };

    auto addLine = [&](float sx, float sy, float ex, float ey) {
        float dx = ex - sx, dy = ey - sy;
        float theta = atan2(dy, dx);
        int steps = std::max(fabs(dx / step_length), fabs(dy / step_length));
        for (int i = 0; i <= steps; ++i)
        {
            float x = sx + dx * i / steps;
            float y = sy + dy * i / steps;
            route.data.push_back(x);
            route.data.push_back(y);
            route.data.push_back(theta);
        }
    };

    // ====== 关键修改：预计算所有拐角切点 ======
    struct CornerData {
        std::pair<float, float> T1;
        std::pair<float, float> T2;
        std::pair<float, float> O;
        float start_angle;
        float end_angle;
        bool reverse_arc;
    };
    std::vector<CornerData> corners;

    // 预计算B、C、D、E四个拐角
    for (int i = 1; i <= 4; ++i) {
        float Ax = waypoints[i-1].first, Ay = waypoints[i-1].second;
        float Bx = waypoints[i].first, By = waypoints[i].second;
        float Cx = waypoints[i+1].first, Cy = waypoints[i+1].second;

        float BAx = Ax - Bx, BAy = Ay - By;
        float BCx = Cx - Bx, BCy = Cy - By;
        float len_BA = normalize(BAx, BAy);
        float len_BC = normalize(BCx, BCy);

        float dot = BAx * BCx + BAy * BCy;
        float theta = acos(dot);
        float half_theta = theta / 2.0;

        float d = ARC_RADIUS / tan(half_theta);
        if (d > 0.4 * len_BA) d = 0.4 * len_BA;
        if (d > 0.4 * len_BC) d = 0.4 * len_BC;

        float T1x = Bx + d * BAx, T1y = By + d * BAy;
        float T2x = Bx + d * BCx, T2y = By + d * BCy;

        float bisectX = (BAx + BCx) / 2.0;
        float bisectY = (BAy + BCy) / 2.0;
        normalize(bisectX, bisectY);
        float O_dist = ARC_RADIUS / sin(half_theta);
        float Ox = Bx + O_dist * bisectX;
        float Oy = By + O_dist * bisectY;

        float start_angle = atan2(T1y - Oy, T1x - Ox);
        float end_angle = atan2(T2y - Oy, T2x - Ox);
        bool reverse_arc = (i == 2 || i == 3);  // C,D点反转圆弧

        corners.push_back({
            {T1x, T1y},
            {T2x, T2y},
            {Ox, Oy},
            start_angle,
            end_angle,
            reverse_arc
        });
    }

    // ====== 重构路径生成逻辑 ======
    // 1. A->B的直线（只画到B的切点T1）
    addLine(waypoints[0].first, waypoints[0].second, 
            corners[0].T1.first, corners[0].T1.second);

    // 2. 处理四个拐角(B,C,D,E)
    for (int i = 0; i < 4; ++i) {
        // 绘制圆弧
        float Ox = corners[i].O.first;
        float Oy = corners[i].O.second;
        float start_angle = corners[i].start_angle;
        float end_angle = corners[i].end_angle;
        bool reverse_arc = corners[i].reverse_arc;
        
        // 角度连续性处理
        if (!reverse_arc && end_angle < start_angle) 
            end_angle += 2 * M_PI;
        if (reverse_arc && start_angle < end_angle) 
            start_angle += 2 * M_PI;

        float angle = start_angle;
        while ((!reverse_arc && angle <= end_angle) || 
               (reverse_arc && angle >= end_angle)) 
        {
            float x = Ox + ARC_RADIUS * cos(angle);
            float y = Oy + ARC_RADIUS * sin(angle);
            float heading = angle + (reverse_arc ? -M_PI/2 : M_PI/2);
            
            // 角度归一化
            if (heading > M_PI) heading -= 2 * M_PI;
            if (heading < -M_PI) heading += 2 * M_PI;
            
            route.data.push_back(x);
            route.data.push_back(y);
            route.data.push_back(heading);
            
            angle += (reverse_arc ? -dtheta : dtheta);
        }

        // 绘制切点T2到下一个目标的直线
        if (i < 3) {  // B,C,D拐角：连接到下一个拐角的T1
            addLine(corners[i].T2.first, corners[i].T2.second,
                    corners[i+1].T1.first, corners[i+1].T1.second);
        } else {      // E拐角：连接到F点
            addLine(corners[i].T2.first, corners[i].T2.second,
                    waypoints[5].first, waypoints[5].second);
        }
    }

    // 3. 处理F->G圆弧
    {
        float sx = waypoints[5].first, sy = waypoints[5].second;
        float ex = waypoints[6].first, ey = waypoints[6].second;
        float radius = fabs(ex - sx);
        float cx = ex, cy = sy;
        for (float theta = 0; theta <= M_PI / 2.0; theta += dtheta)
        {
            float x = cx + radius * sin(theta);
            float y = cy - radius * cos(theta);
            float heading = theta - M_PI / 2;
            route.data.push_back(x);
            route.data.push_back(y);
            route.data.push_back(heading);
        }
    }

    // 4. 处理G->H直线
    addLine(waypoints[6].first, waypoints[6].second, 
            waypoints[7].first, waypoints[7].second);

    // 发布路径
    ros::Rate rate(0.6);
    while (ros::ok())
    {
        pub.publish(route);
        rate.sleep();
    }

    return 0;
}





